#pragma once

#include "stdint.h"
#include "tim.h"
#include "main.h"
#include "mypid.h"

#define M_PI 3.1415926
#define PIEXL2M_X  0.00175489
#define PIEXL2M_Y  0.00170489
#define WALL_DISTANCE  1.0f

typedef struct{
	TIM_HandleTypeDef *htim;
	uint32_t channel;
	int polarity;
	float range;
	uint16_t duty_abs, duty_rela;
	float angle_abs, angle_rela;
	uint16_t duty_max, duty_min;
}Servo_t;


Servo_t* servo_InitStruct(TIM_HandleTypeDef *htim, uint32_t channel, int p, float range, uint16_t duty_max, uint16_t duty_min);
void servo_SetDutyAbs(Servo_t* s, uint16_t duty);
void servo_SetDutyRela(Servo_t* s, int duty);
void servo_SetAngleAbs(Servo_t* s, float angle);
void servo_SetAngleRela(Servo_t* s, float angle);
void servo_CalculateAngle(int pixel_x1, int pixel_y1,int pixel_x2, int pixel_y2,float* x_angle, float* y_angle);

void servo_process1(PID_Positional_t *pid1, PID_Positional_t *pid2, uint16_t section);

extern Servo_t *s1;
extern Servo_t *s2;
